#include <Arduino.h>
#include "BLE112Lib.h"

BGLib ble112((HardwareSerial *)&Serial2, 0, 1);
// ================================================================
// INTERNAL BGLIB CLASS CALLBACK FUNCTIONS
// ================================================================
uint8_t isScanActive = 0;
uint8_t hrble_handle;
bool _connected = false;
bool _disconnected = false;
bool _reseted = false;
bool _discoverable = false;
bool _altura_writed = true;
bool _set_adv_correct = false;
bool _set_mode_correct = false;
bool _notify_correct = false;
bool _attributes_writed = false; 
bool _isBusy = false;;
uint8_t _heart_rate = 0;

void onBusy() {
    _isBusy = true;
}

void onIdle() {
    _isBusy = false;
}

void onTimeout() {
    //Serial.println("***\tTimeout occurred!");
	// ONLY FOR TEST PROGRAMM
	Serial.write(0x2A);
}

void onBeforeTXCommand() {
    // wake module up (assuming here that digital pin 5 is connected to the BLE wake-up pin)
    digitalWrite(BLE_WAKEUP_PIN, HIGH);
	//delay(10);

    // wait for "hardware_io_port_status" event to come through, and parse it (and otherwise ignore it)
    uint8_t *last;
    while (1) {
        ble112.checkActivity();
        last = ble112.getLastEvent();
        if (last[0] == 0x07 && last[1] == 0x00) break;
    }
    
    // give a bit of a gap between parsing the wake-up event and allowing the command to go out
    delayMicroseconds(1000);
}

void onTXCommandComplete() {
    // allow module to return to sleep (assuming here that digital pin 5 is connected to the BLE wake-up pin)
    //digitalWrite(BLE_WAKEUP_PIN, LOW);
	//delay(10);
	_isBusy = false;
}

// ================================================================
// USER-DEFINED BGLIB RESPONSE CALLBACKS
// ================================================================

void my_rsp_system_hello(const ble_msg_system_hello_rsp_t *msg) {
    //Serial.println("<--\tsystem_hello");
	// ONLY FOR TEST PROGRAMM
	Serial.write(0x20);
}

void my_rsp_gap_set_scan_parameters(const ble_msg_gap_set_scan_parameters_rsp_t *msg) {
    Serial.print("<--\tgap_set_scan_parameters: { ");
    Serial.print("result: "); Serial.print((uint16_t)msg -> result, HEX);
    Serial.println(" }");
}

void my_rsp_gap_discover(const ble_msg_gap_discover_rsp_t *msg) {
    Serial.print("<--\tgap_discover: { ");
    Serial.print("result: "); Serial.print((uint16_t)msg -> result, HEX);
    Serial.println(" }");
}

void my_rsp_gap_set_mode(const ble_msg_gap_set_mode_rsp_t *msg){
	#ifdef DEBUG_BT
		Serial.print("\t<<< gap_set_mode: { ");
		Serial.print("result: "); Serial.print((uint16_t)msg -> result, HEX);
	#endif
	if((uint16_t)msg -> result == 0)
	{
		_set_mode_correct = true;
	}
	#ifdef DEBUG_BT
		Serial.println(" }");
	#endif
}

void my_rsp_gap_end_procedure(const ble_msg_gap_end_procedure_rsp_t *msg) {
    Serial.print("<--\tgap_end_procedure: { ");
    Serial.print("result: "); Serial.print((uint16_t)msg -> result, HEX);
    Serial.println(" }");
}

void my_rsp_gap_set_adv_parameters(const ble_msg_gap_set_adv_parameters_rsp_t *msg){
	#ifdef DEBUG_BT
		Serial.print("\t<<< gap_set_adv_parameters: { ");
		Serial.print("result: "); Serial.print((uint16_t)msg -> result, HEX);
	#endif
	if(((uint16_t)msg -> result) == 0){
		_set_adv_correct = true;
	}
	else{
		_set_adv_correct = false;
	}
	#ifdef DEBUG_BT
		Serial.println(" }");
	#endif
}

void my_rsp_gap_connect_direct(const ble_msg_gap_connect_direct_rsp_t *msg){
	Serial.print("<--\tgap_connect_direct: { ");
    Serial.print("result: "); Serial.print((uint16_t)msg -> result, HEX);
	Serial.print(", connection: "); Serial.print((uint8_t)msg -> connection_handle, HEX);
    Serial.println(" }");
	hrble_handle = msg->connection_handle;
}

void my_rsp_attclient_attribute_write( const  ble_msg_attclient_attribute_write_rsp_t * msg){
	Serial.print("<--\tattclient_attribute_write: { ");
    Serial.print("result: "); Serial.print((uint16_t)msg -> result, HEX);
	if(((uint16_t)msg -> result) == 0){
		_notify_correct = true;
	}
	else{
		_notify_correct = false;
	}
    Serial.println(" }");
}

void my_rsp_attclient_find_information(const ble_msg_attclient_find_information_rsp_t * msg){
	Serial.print("<--\tattclient_find_information: { ");
    Serial.print("result: "); Serial.print((uint16_t)msg -> result, HEX);
    Serial.println(" }");
}

void my_rsp_sm_set_bondable_mode(const ble_msg_sm_set_bondable_mode_rsp_t *msg){
	Serial.print("<--\tsm_set_bondable_mode: { ");
    Serial.println(" }");
}

void my_rsp_attribute_write(const ble_msg_attributes_write_rsp_t * msg){
	//Serial.print("<--\trsp_attribute_write: { ");
    //Serial.print("result: "); Serial.print((uint16_t)msg -> result, HEX);
	if((uint16_t)msg -> result == 0)
	{
		_attributes_writed = true;
		_altura_writed = true;
		//Serial.print("; write was successful");
	}
	else
	{
		_altura_writed = false;
		//Serial.print("; Error");
	}
	//Serial.println(" }");
}

void my_rsp_connection_disconnect(const ble_msg_connection_disconnect_rsp_t * msg){
	Serial.print("<--\trsp_connection_disconnect: { ");
    Serial.print("result: "); Serial.print((uint16_t)msg -> result, HEX);
	if((uint16_t)msg -> result == 0)
	{
		Serial.print("; disconnected successful");
		_disconnected=true;
	}
	else
	{
		_altura_writed = false;
		Serial.print("; Error");
	}
	Serial.println(" }");
}

// ================================================================
// USER-DEFINED BGLIB EVENT CALLBACKS
// ================================================================

void my_evt_system_boot(const ble_msg_system_boot_evt_t *msg) {
	#ifdef DEBUG_BT
		Serial.println("\tReseted BLE112-A");
		Serial.print("\t### system_boot: { ");
		Serial.print("major: "); Serial.print(msg -> major, HEX);
		Serial.print(", minor: "); Serial.print(msg -> minor, HEX);
		Serial.print(", patch: "); Serial.print(msg -> patch, HEX);
		Serial.print(", build: "); Serial.print(msg -> build, HEX);
		Serial.print(", ll_version: "); Serial.print(msg -> ll_version, HEX);
		Serial.print(", protocol_version: "); Serial.print(msg -> protocol_version, HEX);
		Serial.print(", hw: "); Serial.print(msg -> hw, HEX);
		Serial.println(" }");
	#endif
	_reseted = true;
}

void my_evt_gap_scan_response(const ble_msg_gap_scan_response_evt_t *msg) {
    Serial.print("###\tgap_scan_response: { ");
    Serial.print("rssi: "); Serial.print(msg -> rssi);
    Serial.print(", packet_type: "); Serial.print((uint8_t)msg -> packet_type, HEX);
    Serial.print(", sender: ");
    // this is a "bd_addr" data type, which is a 6-byte uint8_t array
    for (uint8_t i = 0; i < 6; i++) {
        if (msg -> sender.addr[i] < 16) Serial.write('0');
        Serial.print(msg -> sender.addr[i], HEX);
    }
    Serial.print(", address_type: "); Serial.print(msg -> address_type, HEX);
    Serial.print(", bond: "); Serial.print(msg -> bond, HEX);
    Serial.print(", data: ");
    // this is a "uint8array" data type, which is a length byte and a uint8_t* pointer
    for (uint8_t i = 0; i < msg -> data.len; i++) {
        if (msg -> data.data[i] < 16) Serial.write('0');
        Serial.print(msg -> data.data[i], HEX);
    }
    Serial.println(" }");
	uint8_t status;
	ble112.ble_cmd_gap_connect_direct(msg->sender,0,50,100,250,0);
}

void my_evt_connection_status(const ble_msg_connection_status_evt_t * msg){
	hrble_handle = msg -> connection;
	Serial.println("###\tconnection_status: CONNECTED ");
	//Serial.print(msg -> connection, HEX);
	//ble112.ble_cmd_gap_set_mode(BGLIB_GAP_NON_DISCOVERABLE,BGLIB_GAP_NON_CONNECTABLE);
	//Alarm.delay(100);
	//ble112.ble_cmd_gap_set_adv_parameters(1600,4000,7);
	//Alarm.delay(100);
	//ble112.ble_cmd_gap_set_mode(BGLIB_GAP_NON_DISCOVERABLE,BGLIB_GAP_NON_CONNECTABLE);
	//Alarm.delay(20);
	_connected = true;
	_disconnected = false;
}

void my_evt_connection_disconnected(const ble_msg_connection_disconnected_evt_t * msg){
	hrble_handle = msg -> connection;
	Serial.println("###\tconnection_status: DISCONNECTED ");
	_disconnected = true;
	_connected = false;
}

void my_evt_attclient_attribute_value( const ble_msg_attclient_attribute_value_evt_t * msg){
	Serial.print("###\tattclient_attribute_value: { ");
	Serial.print("hr: ");Serial.print(msg->value.data[1]);
	Serial.println(" }");
	_heart_rate = msg->value.data[1];
    // this is a "uint8array" data type, which is a length byte and a uint8_t* pointer
}

void my_evt_attclient_find_information_found(const ble_msg_attclient_find_information_found_evt_t * msg){
	Serial.print("###\tattclient_find_information_found: { ");
	Serial.print("connection: "); Serial.print(msg->connection);
	Serial.print(", chrhandle: "); Serial.print((uint16_t)msg->chrhandle, HEX);
	Serial.print(", uuid: ");
	// this is a "uint8array" data type, which is a length byte and a uint8_t* pointer
	for (uint8_t i = 0; i < msg -> uuid.len; i++) {
		Serial.print(msg -> uuid.data[i], HEX);
    }
    Serial.println(" }");
}

void my_evt_attclient_procedure_completed(const ble_msg_attclient_procedure_completed_evt_t * msg){
	Serial.print("###\tattclient_procedure_completed: { ");
	Serial.print("connection: "); Serial.print(msg->connection);
	Serial.print(", result: "); Serial.print(msg->result);
	if((msg->result)!=0){
		_notify_correct = false;
	}
	else{
		_notify_correct = true;
	}
	Serial.print(", chrhandle: "); Serial.print((uint16_t)msg->chrhandle, HEX);
	Serial.println(" }");
}

void my_evt_hardware_io_port_status(const struct ble_msg_hardware_io_port_status_evt_t *msg){
	Serial.print("###\tio_port_status: { ");
	Serial.print("timestamp: "); Serial.print((uint16_t)msg->timestamp);
	Serial.print(", port: "); Serial.print(msg->port, HEX);
	Serial.print(", irq: "); Serial.print(msg->irq, HEX);
	Serial.print(", state: "); Serial.print(msg->state, HEX);
	Serial.println(" }");
}

void my_evt_system_protocol_error(const struct ble_msg_system_protocol_error_evt_t * msg){
	//digitalWrite(37, HIGH);
	Serial.print("###\tsystem_protocol_error: { ");
	Serial.print("reason: "); Serial.print(msg->reason, HEX);
	Serial.println(" }");
	ble112.setBusy(false);
	_attributes_writed = true;

	// ONLY FOR TEST PROGRAMM
	//Serial.write(0x2A);
}

bool BLE112Lib::StartBLE112(void){
	// initialize BLE wake-up pin to allow (not force) sleep mode
    //pinMode(BLE_WAKEUP_PIN, OUTPUT);
    //digitalWrite(BLE_WAKEUP_PIN, LOW);

	hear_rate = 0;
	_connected = false;

	// set up internal status handlers
    // (these are technically optional)
    ble112.onBusy = onBusy;
    ble112.onIdle = onIdle;
    ble112.onTimeout = onTimeout;
    
    // ONLY enable these if you are using the <wakeup_pin> parameter in your firmware's hardware.xml file
    //ble112.onBeforeTXCommand = onBeforeTXCommand;
    //ble112.onTXCommandComplete = onTXCommandComplete;

    // set up BGLib response handlers (called almost immediately after sending commands)
    // (these are also technicaly optional)
    ble112.ble_rsp_system_hello = my_rsp_system_hello;
    ble112.ble_rsp_gap_set_scan_parameters = my_rsp_gap_set_scan_parameters;
    ble112.ble_rsp_gap_discover = my_rsp_gap_discover;
	ble112.ble_rsp_gap_set_mode = my_rsp_gap_set_mode;
    ble112.ble_rsp_gap_end_procedure = my_rsp_gap_end_procedure;
	ble112.ble_rsp_gap_connect_direct = my_rsp_gap_connect_direct;
	ble112.ble_rsp_attclient_attribute_write = my_rsp_attclient_attribute_write;
	ble112.ble_rsp_attclient_find_information = my_rsp_attclient_find_information;
	ble112.ble_rsp_gap_set_adv_parameters = my_rsp_gap_set_adv_parameters;
	ble112.ble_rsp_sm_set_bondable_mode = my_rsp_sm_set_bondable_mode;
	ble112.ble_rsp_attributes_write = my_rsp_attribute_write;
	ble112.ble_rsp_connection_disconnect = my_rsp_connection_disconnect;


    // set up BGLib event handlers (called at unknown times)
    ble112.ble_evt_system_boot = my_evt_system_boot;
    ble112.ble_evt_gap_scan_response = my_evt_gap_scan_response;
	ble112.ble_evt_connection_status = my_evt_connection_status;
	ble112.ble_evt_attclient_attribute_value = my_evt_attclient_attribute_value;
	ble112.ble_evt_attclient_find_information_found = my_evt_attclient_find_information_found;
	ble112.ble_evt_attclient_procedure_completed = my_evt_attclient_procedure_completed;
	ble112.ble_evt_connection_disconnected = my_evt_connection_disconnected;
	ble112.ble_evt_hardware_io_port_status = my_evt_hardware_io_port_status;
	ble112.ble_evt_system_protocol_error = my_evt_system_protocol_error;
	#ifdef DEBUG_BT
		Serial.println("\tStarted BLE112-A");
	#endif
}

void BLE112Lib::CheckActivityBLE112(void){
	ble112.checkActivity();
}

void BLE112Lib::SystemHello(void){
	uint8_t status;
	//Serial.println("-->\tsystem_hello");
    ble112.ble_cmd_system_hello();
    #ifdef WAIT_FOR_RESPONSE
		while ((status = ble112.checkActivity(TIME_FOR_WAITING)));
	#endif
    // response should come back within milliseconds
}

void BLE112Lib::SetMode(int type_discover, int type_connect){
	#ifdef DEBUG_BT
		Serial.println("\tSetMode: { }");
	#endif
	uint8_t status;
	ble112.ble_cmd_gap_set_mode(type_discover,type_connect);
	#ifdef WAIT_FOR_RESPONSE
		while ((status = ble112.checkActivity(TIME_FOR_WAITING)));
	#endif
}

void BLE112Lib::Write_Local_Attribute(int attribute, uint8 *value){
	//Serial.println("-->\tWrite_Local_Attribute");
	uint8_t status;
	_attributes_writed = false;
	
	if(attribute == 17){
		//uint8 data = *value;
		ble112.ble_cmd_attributes_write(attribute,0, 2, value);
		//Serial.println(&value[0]);
	}
	else if(attribute == 3){
		
		ble112.ble_cmd_attributes_write(attribute,0, 8, value);
		
	}
	else if(attribute == 22){
		//uint8 data[] = value;
		//uint8 data = *value;
		ble112.ble_cmd_attributes_write(attribute,0, 20, value);
		//ble112.setBusy(false);
		/*
		//value
		for (int i = 0; i < 16 ; i++){
		Serial.print((char)value[i]);
		//Serial.print(" ");
		}
		for (int i = 15; i < 19 ; i++){
		Serial.print(value[i]);
		Serial.print(" ");
		}
		*/
		
	}

	#ifdef WAIT_FOR_RESPONSE
		while ((status = ble112.checkActivity(TIME_FOR_WAITING)));
	#endif
    // response should come back within milliseconds
    // scan response events may happen at any time after this

}

void BLE112Lib::ConnectToHearRateMonitor(void){
	Serial.println("-->\tConnectToHearRateMonitor");
	uint8_t status;
	Serial.println("-->\tgap_set_scan_parameters: { scan_interval: 0xC8, scan_window: 0xC8, active: 1 }");
    ble112.ble_cmd_gap_set_scan_parameters(0xC8, 0xC8, 0);
    while ((status = ble112.checkActivity(1000)));
    // response should come back within milliseconds
    Serial.println("-->\tgap_discover: { mode: 2 (GENERIC) }");
    ble112.ble_cmd_gap_discover(BGLIB_GAP_DISCOVER_GENERIC);
	
	#ifdef WAIT_FOR_RESPONSE
		while ((status = ble112.checkActivity(TIME_FOR_WAITING)));
	#endif
}

void BLE112Lib::SetAdvParameters(uint16 adv_interval_min, uint16 adv_interval_max, uint8 adv_channels){
	ble112.ble_cmd_gap_set_adv_parameters(adv_interval_min, adv_interval_max, adv_channels);
	uint8_t status;
	#ifdef DEBUG_BT
		Serial.println("\tSet Advertisement Mode");
	#endif
	#ifdef WAIT_FOR_RESPONSE
		while ((status = ble112.checkActivity(TIME_FOR_WAITING)));
	#endif

}

bool BLE112Lib::IsConnected(void){
	return _connected;
}

void BLE112Lib::NotifyHeartRate(void){
	Serial.println("-->\tNotifyHeartRate");
	uint8_t status;
	Serial.println("-->\tattclient_find_information");
	//ble112.ble_cmd_attclient_find_information(hrble_handle,0x0001,0xFFFF);
	uint8_t data[2] = {0x01,0x00};
	ble112.ble_cmd_attclient_attribute_write(hrble_handle,0x0013, 2, data);
	while ((status = ble112.checkActivity(TIME_FOR_WAITING)));
                    // response should come back within milliseconds
                    // scan response events may happen at any time after this
}

uint8_t BLE112Lib::GetHeartRate(void){
	hear_rate = _heart_rate;
}

void BLE112Lib::Reset (void){
	#ifdef DEBUG_BT
		Serial.println("\tReset BLE112-A");
	#endif
	_reseted			= false;
	_discoverable		= false;
	_altura_writed		= true;
	_set_adv_correct	= false;
	_set_mode_correct	= false;
	_notify_correct		= false;
	_attributes_writed	= false;
	uint8_t status;
	ble112.ble_cmd_system_reset(0);
	_isBusy = true;
	#ifdef WAIT_FOR_RESPONSE
		while ((status = ble112.checkActivity(TIME_FOR_WAITING)));
	#endif
}

bool BLE112Lib::IsBusy (void){
	return _isBusy;
}

bool BLE112Lib::IsReseted(void){
	return _reseted;
}

void BLE112Lib::Disconect (void){
	Serial.println("-->\tDisconnect");
	ble112.ble_cmd_connection_disconnect(hrble_handle);
}

bool BLE112Lib::IsDisconnected(void){
	return _disconnected;
}

bool BLE112Lib::IsDiscoverable(void){
	return _discoverable;
}

bool BLE112Lib::AlturaIsWrited(void){
	return _altura_writed;
}

bool BLE112Lib::IsNotified(void){
	return _notify_correct;
}

bool BLE112Lib::IsAttributesWrited(void){
	return _attributes_writed;
}

bool BLE112Lib::IsAdvCorrect(void){
	return _set_adv_correct;
}

void BLE112Lib::SetReset(bool state){
	_reseted = state;
}

void BLE112Lib::SetNotify(bool state){
	_notify_correct = state;
}

/*void BLE112Lib::SetAttributesWrited(bool state){
	_attributes_writed = state;
}*/

